#include "optical_flow.h"
#include "timer.h"
#include "spi.h"

#include "module_mgt.h"
#include "vl53lxx.h"
#include "general_gpio.h"
#include "IMU.h"

#define PMW_NCS_PIN					PBout(12)	// 片选引脚
#define OPTICAL_POWER_ENABLE	PBout(0)	// 供电使能引脚

#define RESOLUTION			(0.2131946f)/* 将像素转换为实际距离，1m高度下 1个像素对应的位移，单位cm*/
#define OULIER_LIMIT 		(100)		/*光流像素输出限幅*/
#define VEL_LIMIT			(150.f)		/*光流速度限幅*/
#define CONSTANTS_ONE_G					8.80665f


opFlow_t opFlow;	/*光流*/

#if defined(__CC_ARM) 
	#pragma anon_unions	/*用于支持结构体联合体*/
#endif

//pmw光流模块的数据帧结构体
typedef __packed struct motionBurst_s 
{
	__packed union 
	{
		uint8_t motion;
		__packed struct 
		{
			uint8_t frameFrom0    : 1;
			uint8_t runMode       : 2;
			uint8_t reserved1     : 1;
			uint8_t rawFrom0      : 1;
			uint8_t reserved2     : 2;
			uint8_t motionOccured : 1;
		};
	};

	uint8_t observation;
	int16_t deltaX;	//	x和y方向的运动信息
	int16_t deltaY;

	uint8_t squal;	//信息质量

	uint8_t rawDataSum;
	uint8_t maxRawData;
	uint8_t minRawData;

	uint16_t shutter;
} motionBurst_t;

/* 读取光流数据、并解析到Motion变量 */
static void readMotion(motionBurst_t * motion)
{
	uint8_t address = 0x16;	//数据寄存器地址
	PMW_NCS_PIN = 0;
	delay_us(50);	
	SPI2_PMW_Exchange(1, &address, &address);	//发送地址
	delay_us(50);
	SPI2_PMW_Exchange(sizeof(motionBurst_t), (uint8_t*)motion, (uint8_t*)motion);	//读取地址中的数据
	delay_us(50);
	
	PMW_NCS_PIN = 1;
	
	delay_us(50);

	uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
	realShutter |= (motion->shutter & 0x0ff) << 8;
	motion->shutter = realShutter;
}

static void InitRegisters(void);

/*复位光流数据*/
static void resetOpFlowData(opFlow_t *opf)
{
	for(u8 i=0; i<2; i++)
	{
		opf->pixSum[i] = 0;
		opf->pixComp[i] = 0;
		opf->pixValid[i] = 0;
		opf->pixValidLast[i] = 0;
	}
}


/**
 * @Description 读取光流模块寄存器
 */
static uint8_t registerRead(uint8_t reg)
{
	uint8_t data = 0;

	// 最高位为0 读寄存器
	reg &= ~0x80u;

	PMW_NCS_PIN = 0;
	
	delay_us(50);	
	SPI2_PMW_Exchange(1, &reg, &reg);
	delay_us(500);
	SPI2_PMW_Exchange(1, &data, &data);	
	delay_us(50);
	
	PMW_NCS_PIN = 1;
	
	delay_us(200);

	return data;
}


/**
 * @Description 写光流模块寄存器的值
 */
static void registerWrite(uint8_t reg, uint8_t value)
{
	// 最高位为1 写寄存器
	reg |= 0x80u;
	
	PMW_NCS_PIN = 0;
	
	delay_us(50);
	SPI2_PMW_Exchange(1, &reg, &reg);
	delay_us(50);
	SPI2_PMW_Exchange(1, &value, &value);
	delay_us(50);

	PMW_NCS_PIN = 1;
	
	delay_us(200);
}


 
/**
 * @Description 获取光流的速度、位移数据
 */
bool getOpFlowData(opFlow_t *opFlow, float dt)
{
	static u8 cnt = 0;
	float height = RT_Info.LASER_Alt;	//使用激光测距的高度数据

	if(opFlow->isOpFlowOk && height<4.0f)	/*4m范围内，光流可用*/
	{
		cnt= 0;
		opFlow->isDataValid = true;
		
		//计算高度比例
		float coeff = RESOLUTION * height;
		//计算倾角
		float tanRoll = tanf(RT_Info.Roll * DEG2RAD);
		float tanPitch = tanf(RT_Info.Pitch * DEG2RAD);
		
		opFlow->pixComp[X] = 480.f * tanPitch;	/*像素补偿，负方向*/
		opFlow->pixComp[Y] = 480.f * tanRoll;
		opFlow->pixValid[X] = (opFlow->pixSum[X] + opFlow->pixComp[X]);	/*实际输出像素*/
		opFlow->pixValid[Y] = (opFlow->pixSum[Y] + opFlow->pixComp[Y]);		
		
		if(height < 0.05f)	/*光流测量范围大于5cm*/
		{
			coeff = 0.0f;
		}
		opFlow->deltaPos[X] = coeff * (opFlow->pixValid[X] - opFlow->pixValidLast[X]);	/*2帧之间位移变化量，单位cm*/
		opFlow->deltaPos[Y] = coeff * (opFlow->pixValid[Y] - opFlow->pixValidLast[Y]);	
		opFlow->pixValidLast[X] = opFlow->pixValid[X];	
		opFlow->pixValidLast[Y] = opFlow->pixValid[Y];
		opFlow->deltaVel[X] = opFlow->deltaPos[X] / dt;	/*速度 cm/s*/
		opFlow->deltaVel[Y] = opFlow->deltaPos[Y] / dt;
		
		
		opFlow->velLpf[X] += (opFlow->deltaVel[X] - opFlow->velLpf[X]) * 0.15f;	/*速度低通 cm/s*/
		opFlow->velLpf[Y] += (opFlow->deltaVel[Y] - opFlow->velLpf[Y]) * 0.15f;	/*速度低通 cm/s*/
	
		opFlow->velLpf[X] = constrainf(opFlow->velLpf[X], -VEL_LIMIT, VEL_LIMIT);	/*速度限幅 cm/s*/
		opFlow->velLpf[Y] = constrainf(opFlow->velLpf[Y], -VEL_LIMIT, VEL_LIMIT);	/*速度限幅 cm/s*/
	
		opFlow->posSum[X] += opFlow->deltaPos[X];	/*累积位移 cm*/
		opFlow->posSum[Y] += opFlow->deltaPos[Y];	/*累积位移 cm*/

	}
	else if(opFlow->isDataValid == true)
	{
		//超过4m、清除光流数据
		if(cnt++ > 100)	
		{
			cnt = 0;
			opFlow->isDataValid = false;
		}	
		resetOpFlowData(opFlow);	
	}
	
	return opFlow->isOpFlowOk;	/*返回光流状态*/
}


/**
 * @Description 初始化光流模块
 */
void OpticalFlow_Init(void)
{

	GPIO_InitTypeDef GPIO_InitStructure;

	//初始化CS引脚	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);//使能时钟
		
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;	
	GPIO_Init(GPIOB, &GPIO_InitStructure);			

	resetOpFlowData(&opFlow);
	opFlow.isOpFlowOk = true;				
	
	delay_ms(50);
	
	PMW_NCS_PIN = 1;
	SPI2_to_PMW_Init();
	delay_ms(40);

	uint8_t chipId = registerRead(0);
	uint8_t invChipId = registerRead(0x5f);

	if(chipId == 0x00 || chipId != (uint8_t)(~invChipId)) {
		while(1) {	// 低音长鸣
			BEEP_ON;
			delay_ms(5);
			BEEP_OFF;
			delay_ms(5);
		}		
	}
	// 上电复位
	registerWrite(0x3a, 0x5a);
	delay_ms(5);

	InitRegisters();
	delay_ms(5);
	
}



/**
 * @Description 初始化和光流寄存器
 */
static void InitRegisters(void)
{	
	registerWrite(0x7F, 0x00);
	registerWrite(0x61, 0xAD);
	registerWrite(0x7F, 0x03);
	registerWrite(0x40, 0x00);
	registerWrite(0x7F, 0x05);
	registerWrite(0x41, 0xB3);
	registerWrite(0x43, 0xF1);
	registerWrite(0x45, 0x14);
	registerWrite(0x5B, 0x32);
	registerWrite(0x5F, 0x34);
	registerWrite(0x7B, 0x08);
	registerWrite(0x7F, 0x06);
	registerWrite(0x44, 0x1B);
	registerWrite(0x40, 0xBF);
	registerWrite(0x4E, 0x3F);
	registerWrite(0x7F, 0x08);
	registerWrite(0x65, 0x20);
	registerWrite(0x6A, 0x18);
	registerWrite(0x7F, 0x09);
	registerWrite(0x4F, 0xAF);
	registerWrite(0x5F, 0x40);
	registerWrite(0x48, 0x80);
	registerWrite(0x49, 0x80);
	registerWrite(0x57, 0x77);
	registerWrite(0x60, 0x78);
	registerWrite(0x61, 0x78);
	registerWrite(0x62, 0x08);
	registerWrite(0x63, 0x50);
	registerWrite(0x7F, 0x0A);
	registerWrite(0x45, 0x60);
	registerWrite(0x7F, 0x00);
	registerWrite(0x4D, 0x11);
	registerWrite(0x55, 0x80);
	registerWrite(0x74, 0x1F);
	registerWrite(0x75, 0x1F);
	registerWrite(0x4A, 0x78);
	registerWrite(0x4B, 0x78);
	registerWrite(0x44, 0x08);
	registerWrite(0x45, 0x50);
	registerWrite(0x64, 0xFF);
	registerWrite(0x65, 0x1F);
	registerWrite(0x7F, 0x14);
	registerWrite(0x65, 0x67);
	registerWrite(0x66, 0x08);
	registerWrite(0x63, 0x70);
	registerWrite(0x7F, 0x15);
	registerWrite(0x48, 0x48);
	registerWrite(0x7F, 0x07);
	registerWrite(0x41, 0x0D);
	registerWrite(0x43, 0x14);
	registerWrite(0x4B, 0x0E);
	registerWrite(0x45, 0x0F);
	registerWrite(0x44, 0x42);
	registerWrite(0x4C, 0x80);
	registerWrite(0x7F, 0x10);
	registerWrite(0x5B, 0x02);
	registerWrite(0x7F, 0x07);
	registerWrite(0x40, 0x41);
	registerWrite(0x70, 0x00);

	delay_ms(10); // delay 10ms

	registerWrite(0x32, 0x44);
	registerWrite(0x7F, 0x07);
	registerWrite(0x40, 0x40);
	registerWrite(0x7F, 0x06);
	registerWrite(0x62, 0xF0);
	registerWrite(0x63, 0x00);
	registerWrite(0x7F, 0x0D);
	registerWrite(0x48, 0xC0);
	registerWrite(0x6F, 0xD5);
	registerWrite(0x7F, 0x00);
	registerWrite(0x5B, 0xA0);
	registerWrite(0x4E, 0xA8);
	registerWrite(0x5A, 0x50);
	registerWrite(0x40, 0x80);
	
}


/*
* @Description:卡尔曼滤波融合光流与加速度计数据，得到水平速度
*/
void OpticalFlow_Estimation(float OpticalFlow_dt)
{
	float Ultrasonic = RT_Info.LASER_Alt;
	float flow_x = opFlow.deltaVel[X]/100.f;
	float flow_y = opFlow.deltaVel[Y]/100.f;
	
	static float Z_height = 10.0f;
	static float Acc_bias = 0.8f;
	static float z_est[2] = {0.0f, 0.0f};			 //Z轴的高度和速度
	static float accel_ned[3] = {0.0f, 0.0f, 0.0f};	 //地理坐标系下的加速度数据
	static float accel_bias[3] = {0.0f, 0.0f, 0.0f}; //机体坐标系下的加速度偏移量
	static float corr_Ultra = 0.0f;	//校准高度数据
	float accel_bias_corr[3] = {0.0f, 0.0f, 0.0f};	//加速度偏移校准
	float accel_now[3] = {0.0f, 0.0f, 0.0f};	//当前的机体坐标系下加速度

	accel_now[0] = Accer[0];
	accel_now[1] = Accer[1];
	accel_now[2] = Accer[2];	
	
	/* 融合高度 */
	//高度校准=测量高度-预测高度？
	corr_Ultra = 0 - Ultrasonic - z_est[0];
	//加速度除去偏移量
	accel_now[0] -= accel_bias[0];
	accel_now[1] -= accel_bias[1];
	accel_now[2] -= accel_bias[2];

	//计算NED坐标系下的加速度
	for (int i = 0; i < 3; i++)
	{
		accel_ned[i] = 0.0f;
		for (int j = 0; j < 3; j++)
		{
			accel_ned[i] += RDrone_R[i][j] * accel_now[j];
		}
	}	
	//z轴上加上地球重力
	accel_ned[2] += CONSTANTS_ONE_G;
	//计算校准后的z轴加速度偏移 为啥这么做
	accel_bias_corr[2] -= corr_Ultra * Z_height * Z_height;	
	//将校准后的加速度偏移 转化为 机体坐标系下的加速度偏移
	for (int i = 0; i < 3; i++)
	{
		float c = 0.0f;
		for (int j = 0; j < 3; j++)
		{
			c += RDrone_R[j][i] * accel_bias_corr[j];
		}
		accel_bias[i] += c * Acc_bias * OpticalFlow_dt;
	}
	//Z轴预测
	inertial_filter_predict(OpticalFlow_dt, z_est, accel_ned[2]);
	inertial_filter_correct(corr_Ultra, OpticalFlow_dt, z_est, 0, Z_height);

	RT_Info.US100_Alt = -z_est[0];
	RT_Info.US100_Alt_V = -z_est[1];	
	
	/* x轴 */
	static float OpticalFlowx_Velocity;
	static float Q_OpticalFlowxVelocity = 0.001, Q_xbias = 0.001;
	static float R_OpticalFlowx = 0.1;
	static float OpticalFlowx_Velocityerr, OpticalFlowxbias;
	static float OpticalFlowxPCt_0 = 0, OpticalFlowxPCt_1 = 0, OpticalFlowxE = 0;
	static float OpticalFlowxK_0 = 0, OpticalFlowxK_1 = 0, OpticalFlowxt_0 = 0, OpticalFlowxt_1 = 0;
	static char OpticalFlowxC_0 = 1;
	static float OpticalFlowxPdot[4], OpticalFlowxP[2][2];

	//x轴加速度是否需要矫正
	OpticalFlowx_Velocity += (Accer[0] - OpticalFlowxbias) * OpticalFlow_dt;
	
	OpticalFlowxPdot[0] = Q_OpticalFlowxVelocity - OpticalFlowxP[0][1] - OpticalFlowxP[1][0];
	OpticalFlowxPdot[1] = -OpticalFlowxP[1][1];
	OpticalFlowxPdot[2] = -OpticalFlowxP[1][1];
	OpticalFlowxPdot[3] = Q_xbias;

	OpticalFlowxP[0][0] += OpticalFlowxPdot[0] * OpticalFlow_dt;
	OpticalFlowxP[0][1] += OpticalFlowxPdot[1] * OpticalFlow_dt;
	OpticalFlowxP[1][0] += OpticalFlowxPdot[2] * OpticalFlow_dt;
	OpticalFlowxP[1][1] += OpticalFlowxPdot[3] * OpticalFlow_dt;

	OpticalFlowxPCt_0 = OpticalFlowxC_0 * OpticalFlowxP[0][0];
	OpticalFlowxPCt_1 = OpticalFlowxC_0 * OpticalFlowxP[1][0];
	OpticalFlowxE = R_OpticalFlowx + OpticalFlowxC_0 * OpticalFlowxPCt_0;
	OpticalFlowxK_0 = OpticalFlowxPCt_0 / OpticalFlowxE;
	OpticalFlowxK_1 = OpticalFlowxPCt_1 / OpticalFlowxE;

	OpticalFlowx_Velocityerr = flow_x - OpticalFlowx_Velocity;
	OpticalFlowx_Velocity += OpticalFlowxK_0 * OpticalFlowx_Velocityerr;
	OpticalFlowxbias += OpticalFlowxK_1 * OpticalFlowx_Velocityerr;

	OpticalFlowxt_0 = OpticalFlowxPCt_0;
	OpticalFlowxt_1 = OpticalFlowxC_0 * OpticalFlowxP[0][1];

	OpticalFlowxP[0][0] -= OpticalFlowxK_0 * OpticalFlowxt_0;
	OpticalFlowxP[0][1] -= OpticalFlowxK_0 * OpticalFlowxt_1;
	OpticalFlowxP[1][0] -= OpticalFlowxK_1 * OpticalFlowxt_0;
	OpticalFlowxP[1][1] -= OpticalFlowxK_1 * OpticalFlowxt_1;	
	
	RT_Info.FlowX_V = OpticalFlowx_Velocity;
	
	/* y轴 */
	static float OpticalFlowy_Velocity;
	static float Q_OpticalFlowyVelocity = 0.001, Q_ybias = 0.001;
	static float R_OpticalFlowy = 0.1;
	static float OpticalFlowy_Velocityerr, OpticalFlowybias;
	static float OpticalFlowyPCt_0 = 0, OpticalFlowyPCt_1 = 0, OpticalFlowyE = 0;
	static float OpticalFlowyK_0 = 0, OpticalFlowyK_1 = 0, OpticalFlowyt_0 = 0, OpticalFlowyt_1 = 0;
	static char OpticalFlowyC_0 = 1;
	static float OpticalFlowyPdot[4], OpticalFlowyP[2][2];

	OpticalFlowy_Velocity += (Accer[1] - OpticalFlowybias) * OpticalFlow_dt;

	OpticalFlowyPdot[0] = Q_OpticalFlowyVelocity - OpticalFlowyP[0][1] - OpticalFlowyP[1][0];
	OpticalFlowyPdot[1] = -OpticalFlowyP[1][1];
	OpticalFlowyPdot[2] = -OpticalFlowyP[1][1];
	OpticalFlowyPdot[3] = Q_ybias;

	OpticalFlowyP[0][0] += OpticalFlowyPdot[0] * OpticalFlow_dt;
	OpticalFlowyP[0][1] += OpticalFlowyPdot[1] * OpticalFlow_dt;
	OpticalFlowyP[1][0] += OpticalFlowyPdot[2] * OpticalFlow_dt;
	OpticalFlowyP[1][1] += OpticalFlowyPdot[3] * OpticalFlow_dt;

	OpticalFlowyPCt_0 = OpticalFlowyC_0 * OpticalFlowyP[0][0];
	OpticalFlowyPCt_1 = OpticalFlowyC_0 * OpticalFlowyP[1][0];
	OpticalFlowyE = R_OpticalFlowy + OpticalFlowyC_0 * OpticalFlowyPCt_0;
	OpticalFlowyK_0 = OpticalFlowyPCt_0 / OpticalFlowyE;
	OpticalFlowyK_1 = OpticalFlowyPCt_1 / OpticalFlowyE;

	OpticalFlowy_Velocityerr = flow_y - OpticalFlowy_Velocity;
	OpticalFlowy_Velocity += OpticalFlowyK_0 * OpticalFlowy_Velocityerr;
	OpticalFlowybias += OpticalFlowyK_1 * OpticalFlowy_Velocityerr;

	OpticalFlowyt_0 = OpticalFlowyPCt_0;
	OpticalFlowyt_1 = OpticalFlowyC_0 * OpticalFlowyP[0][1];

	OpticalFlowyP[0][0] -= OpticalFlowyK_0 * OpticalFlowyt_0;
	OpticalFlowyP[0][1] -= OpticalFlowyK_0 * OpticalFlowyt_1;
	OpticalFlowyP[1][0] -= OpticalFlowyK_1 * OpticalFlowyt_0;
	OpticalFlowyP[1][1] -= OpticalFlowyK_1 * OpticalFlowyt_1;

	RT_Info.FlowY_V = OpticalFlowy_Velocity;	
}



/*
* @Description:更新水平速度、位移
*/
void Horizontal_Update(void)
{

	static unsigned int OpticalFlowtPre=0;	//上一次计算时间戳
	motionBurst_t Motion;
	static u16 err = 0;	
	
	unsigned int OpticalFlowt = get_micros();	//获取当前时间、计算出经过的时间dt
	unsigned int OpticalFlow_dt = (OpticalFlowtPre>0)?((OpticalFlowt-OpticalFlowtPre)/1000000.0f):1.0f;
	OpticalFlowtPre = OpticalFlowt;	
	
	readMotion(&Motion);
	
	//光流最大最小原始数据不能都是0
	if(Motion.minRawData == 0 && Motion.maxRawData == 0)
	{
		//连续100次数据都是0、模块出错
		if(err++ > 100 && opFlow.isOpFlowOk == true)
		{
			err = 0;
			opFlow.isOpFlowOk = false;	
		}		
	}else
	{
		err = 0;
		opFlow.isOpFlowOk = true;	
	}	
	/*连续2帧之间的像素变化，根据实际安装方向调整 (pitch:x)  (roll:y)*/
	int16_t pixelDx = -Motion.deltaX;
	int16_t pixelDy = Motion.deltaY;	

	//限制幅度 并将像素变化累计
	if (ABS(pixelDx) < OULIER_LIMIT && ABS(pixelDy) < OULIER_LIMIT) 
	{
		opFlow.pixSum[X] += pixelDx;
		opFlow.pixSum[Y] += pixelDy;
	}	
	
	getOpFlowData(&opFlow,OpticalFlow_dt);
	OpticalFlow_Estimation(OpticalFlow_dt);
}
